#include "AmoreRobotDrive.h"
#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class RobotDemo : public SimpleRobot
{
	AmoreRobotDrive *myRobot; 	// robot drive system
	Joystick *joystick; 	  	// only joystick
	DriverStation *ds; 			// driver station

public:
	RobotDemo(void) {
		Accelerometer(225);		// Initialize the accelerometer with interrupt handler 225
		
		ds = DriverStation::GetInstance();
		myRobot = new AmoreRobotDrive(1, 2); // custom robot drive base
		joystick = new Joystick(1); // create the joysticks
		GetWatchdog().SetExpiration(100);
	}

	/**
	 * Dummy autonomous mode for testing:
	 * 		drive left & right motors for 2 seconds then stop
	 */
	void Autonomous(void) {
		GetWatchdog().SetEnabled(false);
		myRobot->Drive(0.5, 0.0); 	// drive forwards half speed
		Wait(2000); 				//    for 2 seconds
		myRobot->Drive(0.0, 0.0); 	// stop robot
		Wait(2000);					//    for 2 seconds
		myRobot->Drive(0.5, 0.2);   // turn right
		myRobot->Drive(0.5, 0.0);	// drive forward
		
		// more driving
	}

	/**
	 * Runs the motors with arcade steering. 
	 */
	void OperatorControl(void) {
		GetWatchdog().SetEnabled(true);
		
		while (IsOperatorControl()) {
			GetWatchdog().Feed();
			//ds.GetStickAxis(stick, axis);
			
			myRobot->ArcadeDrive(joystick);	// drive with arcade style (use right stick)
			
			if (joystick->) {
				
			}
		}
	}
};

START_ROBOT_CLASS(RobotDemo);